#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：刘骏轩2025讯飞
@File ：guoLineControl.py
@Author ：ljx
@Date ：2025/7/25 下午9:30
'''

from GetCvPicture import PicGetter
import cv2
import rospy
from geometry_msgs.msg import Twist
from guoLineFind import LineFinder
import threading
import numpy as np
from std_srvs.srv import Trigger, TriggerResponse
import time


class PD_Controler:
    def __init__(self):
        self.KP = 0.85
        self.KD = 0.45
        self.last_error = 0
        self.last_last_error = 0
        self.max_output = 2.0
        self.target = 0
        self.output = 0

    def Revise(self, now):
        if now is None:
            return None
        """
        PD 控制角速度
        :param now:当前值，now = 中点坐标的平均值减去320  now > 0 :则说明中线在小车的靠右位置 小车需右转 , now < 0 : 中线在小车的靠左位置 , 小车需左转
        :return: 控制输出
        """
        # 采用增量式PD
        # 角速度为负时,是顺时针，即向右偏 ; 为正时,是逆时针，即向左偏
        error = self.target - now
        self.output += self.KP * (error - self.last_error) + self.KD * (
                    error + self.last_last_error - 2 * self.last_error)

        self.output = np.clip(self.output, -self.max_output, self.max_output)
        self.last_last_error = self.last_error
        self.last_error = error

        return self.output


class Line_Controler:
    def __init__(self):
        self.cap = PicGetter()
        self.PD_control = PD_Controler()
        self.finder = LineFinder()
        # 表示是否在巡线
        self.active = False
        self.cmd_pub = None
        self.control_thread = None
        # 发布速度消息
        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

        self.emergency = False

    def start(self):
        """
        这个服务只会启动一次，所以没有避免考虑过多，比如说self.image_sub 和 self.cmd_pub 重复申请的问题
        :return:
        """
        if self.active:
            rospy.logwarn("Controller is already running")
            return
        self.active = True
        # self.control_thread = threading.Thread(target=self.run)
        # self.control_thread.daemon = True  # 设置为守护线程
        # self.control_thread.start()
        # 开始订阅图片
        self.cap.start()
        rospy.loginfo("Control thread started")
        self.run()

    def run(self):
        # rate = rospy.Rate(20)  # 25Hz

        while not rospy.is_shutdown() and self.active:
            img = self.cap.Get()
            if img is None:
                continue
            try:
                msg = Twist()
                error = self.finder.solve(img, 'test')
                print("area:", self.finder.area)
                if self.finder.area == "transition":
                    if self.finder.transition_Counts == 0:
                        msg.linear.x = 0.15
                        if self.finder.outLine == "right":
                            msg.angular.z = 0.35
                        else:
                            msg.angular.z = -0.38
                    elif self.finder.transition_Counts == 1:
                        if self.finder.outLine == "right":
                            msg.linear.x = 0.05
                            msg.angular.z = 0.68
                        else:
                            msg.angular.z = -0.2
                            start = time.time()
                            rate = rospy.Rate(10)
                            while time.time() - start < 0.05:
                                self.cmd_pub.publish(msg)
                                rate.sleep()
                            msg.linear.x = 0.08
                            msg.angular.z = -0.72

                        start = time.time()
                        rate = rospy.Rate(20)
                        while time.time() - start < 0.1:
                            self.cmd_pub.publish(msg)
                            rate.sleep()
                elif self.finder.area == "transition2":
                    msg.linear.x = 0.35
                    if self.finder.outLine == "right":
                        msg.angular.z = 0.92
                    else:
                        msg.angular.z = -0.92
                    start = time.time()
                    rate = rospy.Rate(20)
                    while time.time() - start < 0.18:
                        self.cmd_pub.publish(msg)
                        rate.sleep()
                elif self.finder.area == "end":
                    msg.linear.x = 0.1
                    msg.linear.y = -0.07
                    if not self.emergency:
                        self.emergency = True
                        rospy.loginfo("Attention!!!")
                elif self.finder.area == "stop":
                    rospy.loginfo("Line Finish")
                    break
                else:
                    if self.emergency:
                        break
                    msg.linear.x = 0.25
                    if error is not None:
                        output = self.PD_control.Revise(error) if abs(error) > 0.05 else 0
                        if output is None:
                            continue
                        rospy.loginfo(f"output:{output}")
                        msg.angular.z = output

                self.cmd_pub.publish(msg)

            except Exception as e:
                rospy.logerr(f"控制出错: {e}")
                rospy.sleep(0.1)  # 出错时稍作暂停

            # rate.sleep()

        # 停止机器人
        stop_msg = Twist()
        self.cmd_pub.publish(stop_msg)

        self.active = False

        rospy.loginfo("Control thread stopped")


class LineService:
    def __init__(self):
        self.control = Line_Controler()
        self.service = None

    def doRequest(self, req):
        if self.control.active:
            return TriggerResponse(False, "Already running")

        # 运行控制逻辑
        self.control.start()

        return TriggerResponse(True, "Line control started")

    def start(self):
        rospy.loginfo("Line Service is ready")
        self.service = rospy.Service("LinePatrolService", Trigger, self.doRequest)


if __name__ == '__main__':
    rospy.init_node('LinePatrol', anonymous=True)

    service = LineService()

    service.start()

    rospy.spin()
